#include <ros/ros.h>

#include <tf/transform_datatypes.h>
#include <interactive_markers/interactive_marker_server.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>

#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionIK.h>
#include <kinematics_msgs/GetConstraintAwarePositionIK.h>

#include <arm_navigation_msgs/SetPlanningSceneDiff.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/JointTrajectory.h>

class WAMArmMarkerServer
{
	
  private: 
  
    ros::NodeHandle nh;
	ros::Timer arm_timer;
	tf::TransformListener tf_listener;
	
	std::string tip_link;
    std::string root_link;
    
    double move_time;
    
    // create an interactive marker server on the topic namespace simple_marker
    interactive_markers::InteractiveMarkerServer server;
    
    ros::ServiceClient query_client;
    ros::ServiceClient ik_client;
	
	// define the service messages
    kinematics_msgs::GetKinematicSolverInfo::Request request;
    kinematics_msgs::GetKinematicSolverInfo::Response response;
    
    // create a ResetMarker service server
    ros::ServiceServer resetMarker_server;
    
    ros::Publisher arm_com_pub;
    
    ros::ServiceClient set_planning_scene_diff_client;
	
  public:
	WAMArmMarkerServer()
	: server("wam_arm_marker_server")
	{
		
	  //ros::init(argc, argv, "wam_tcp_interactive_marker");
	  
	  // Get general arm parameters
      nh.param<std::string>("root_link", root_link, "/base_link");
	  nh.param<std::string>("tip_link", tip_link, "/wam_tcp");
      nh.param<double>("move_time", move_time, 2.0);

	  // create an interactive marker for our server
	  visualization_msgs::InteractiveMarker int_marker;
	  int_marker.header.frame_id = "/base_link";
	  int_marker.name = "arm_marker";
	  int_marker.scale = 0.25;
	  
	  static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
      set_planning_scene_diff_client = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
	  
	  arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
      arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;

	  if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
		ROS_WARN("Can't get planning scene");
	  }
	  
	  arm_timer.start();

	  // create a grey box marker
	  visualization_msgs::Marker box_marker;
	  box_marker.type = visualization_msgs::Marker::CUBE;
	  box_marker.scale.x = 0.02; //0.045;
	  box_marker.scale.y = 0.02;
	  box_marker.scale.z = 0.02;

	  box_marker.color.r = 0.5;
	  box_marker.color.g = 0.5;
	  box_marker.color.b = 0.5;
	  box_marker.color.a = 1.0;

	  // create a non-interactive control which contains the box
	  visualization_msgs::InteractiveMarkerControl box_control;
	  box_control.always_visible = true;
	  box_control.markers.push_back( box_marker );

	  // add the control to the interactive marker
	  int_marker.controls.push_back( box_control );

	  // create the controls which will move the box
	  // this control does not contain any markers,
	  // which will cause RViz to insert two arrows
	  visualization_msgs::InteractiveMarkerControl control;

	  control.orientation.w = 1;
	  control.orientation.x = 1;
	  control.orientation.y = 0;
	  control.orientation.z = 0;
	  control.always_visible = false;
	  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
	  // add the control to the interactive marker
	  int_marker.controls.push_back(control);
	  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
	  // add the control to the interactive marker
	  int_marker.controls.push_back(control);

	  control.orientation.w = 1;
	  control.orientation.x = 0;
	  control.orientation.y = 1;
	  control.orientation.z = 0;
	  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
	  int_marker.controls.push_back(control);
	  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
	  int_marker.controls.push_back(control);

	  control.orientation.w = 1;
	  control.orientation.x = 0;
	  control.orientation.y = 0;
	  control.orientation.z = 1;
	  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
	  int_marker.controls.push_back(control);
	  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
	  int_marker.controls.push_back(control);
	  

	  // add the interactive marker to our collection &
	  // tell the server to call processFeedback() when feedback arrives for it
	  server.insert(int_marker, boost::bind(&WAMArmMarkerServer::processArmFeedback, this, _1 ));

	  // 'commit' changes and send to all clients
	  server.applyChanges();
	  
	  sleep(2);
	  resetMarker();
	  
	  ros::service::waitForService("/gwam_wam_chain_kinematics/get_ik_solver_info");
	  ros::service::waitForService("/gwam_wam_chain_kinematics/get_ik");
	  
	  query_client = nh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>("/gwam_wam_chain_kinematics/get_ik_solver_info");
	  ik_client = nh.serviceClient<kinematics_msgs::GetPositionIK>("/gwam_wam_chain_kinematics/get_ik");
		
	  arm_com_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/iri_ros_controller/command", 1);	
	  
	  resetMarker_server = nh.advertiseService("/wam_arm_marker_server/reset_marker", &WAMArmMarkerServer::resetMarker, this);	
  }

void processArmFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
    if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN && feedback->marker_name == "arm_marker")
    {
      arm_timer.stop();
    }
    if (feedback->event_type != visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP || feedback->marker_name != "arm_marker")
      return;
   
    bool sent = sendTrajectoryCommand(feedback);
    
    if (sent){
		changeMarkerColor(0, 1, 0);
	} else {
		changeMarkerColor(1, 0, 0);
	}
}

bool sendTrajectoryCommand(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
	
	bool commandOk = false;

    if(query_client.call(request,response))
    {
		/* Check
		for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
		{
			ROS_ERROR("Joint: %d %s",i,response.kinematic_solver_info.joint_names[i].c_str());
		}
		*/ 
		
		kinematics_msgs::GetPositionIK::Request  gpik_req;
		kinematics_msgs::GetPositionIK::Response gpik_res;
		gpik_req.timeout = ros::Duration(5.0);
		gpik_req.ik_request.ik_link_name = "wam_tcp";

		gpik_req.ik_request.pose_stamped.header.frame_id = "base_link";
		gpik_req.ik_request.pose_stamped.pose.position.x = feedback->pose.position.x;
		gpik_req.ik_request.pose_stamped.pose.position.y = feedback->pose.position.y;
		gpik_req.ik_request.pose_stamped.pose.position.z = feedback->pose.position.z;

		gpik_req.ik_request.pose_stamped.pose.orientation.x = feedback->pose.orientation.x;
		gpik_req.ik_request.pose_stamped.pose.orientation.y = feedback->pose.orientation.y;
		gpik_req.ik_request.pose_stamped.pose.orientation.z = feedback->pose.orientation.z;
		gpik_req.ik_request.pose_stamped.pose.orientation.w = feedback->pose.orientation.w;
		
		gpik_req.ik_request.ik_seed_state.joint_state.position.resize(response.kinematic_solver_info.joint_names.size());
		gpik_req.ik_request.ik_seed_state.joint_state.name = response.kinematic_solver_info.joint_names;
		for(unsigned int i=0; i< response.kinematic_solver_info.joint_names.size(); i++)
		{
			gpik_req.ik_request.ik_seed_state.joint_state.position[i] = (response.kinematic_solver_info.limits[i].min_position + response.kinematic_solver_info.limits[i].max_position)/2.0;
		}
		if(ik_client.call(gpik_req, gpik_res))
		{
			if(gpik_res.error_code.val == gpik_res.error_code.SUCCESS){
				for(unsigned int i=0; i < gpik_res.solution.joint_state.name.size(); i ++){
					ROS_INFO("Joint: %s %f",gpik_res.solution.joint_state.name[i].c_str(),gpik_res.solution.joint_state.position[i]);
				}
				// Send command to the joints
				trajectory_msgs::JointTrajectory jt;
				trajectory_msgs::JointTrajectoryPoint pt;

				jt.header.stamp = ros::Time::now();
				jt.header.frame_id = "base_link";
				
				jt.joint_names.push_back(gpik_res.solution.joint_state.name[0].c_str());
				jt.joint_names.push_back(gpik_res.solution.joint_state.name[1].c_str());
				jt.joint_names.push_back(gpik_res.solution.joint_state.name[2].c_str());
				jt.joint_names.push_back(gpik_res.solution.joint_state.name[3].c_str());
				jt.joint_names.push_back(gpik_res.solution.joint_state.name[4].c_str());
				jt.joint_names.push_back(gpik_res.solution.joint_state.name[5].c_str());
				jt.joint_names.push_back(gpik_res.solution.joint_state.name[6].c_str());

				pt.positions.resize(7);

				for (int i = 0; i < 7; i++) {
					pt.positions[i] = gpik_res.solution.joint_state.position[i];
				 }
				 
				 pt.time_from_start = ros::Duration(20.0);

				 jt.points.push_back(pt);

				 arm_com_pub.publish(jt);

				 ROS_WARN("Move command sent!");
				 
				 commandOk = true;
				
			}else{
				
				 //ROS_ERROR("Inverse kinematics failed");
				 commandOk = false;
			}
		}
		
	else{
		
		//ROS_ERROR("Inverse kinematics service call failed");
		commandOk = false;
		
	}
	
	/// Reset the PlanningScene to avoid possible errors
    arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
    arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
	
    if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
		ROS_WARN("Can't get planning scene");
	}	
		
	}
	else
	{
		ROS_ERROR("Could not call query service");
		commandOk = false;
	}
	
	return commandOk;

}
	
    
void getTransformedPose(const std::string& source_frame, const geometry_msgs::Pose& source_pose,
                          const std::string& target_frame, geometry_msgs::Pose& target_pose,
                          const ros::Time& time)
{
    tf::Pose bt_source_pose;
    
    tf::poseMsgToTF(source_pose, bt_source_pose);
    
    tf::Stamped<tf::Pose> posein(bt_source_pose, time, source_frame);
    tf::Stamped<tf::Pose> poseout;
    
    try 
    {
      ros::Duration timeout(10.0);
      
      // Get base_link transform
      tf_listener.waitForTransform(target_frame, source_frame,
                                    time, timeout);
      tf_listener.transformPose(target_frame, posein, poseout);
      
      
    }
    catch (tf::TransformException& ex) {
      ROS_WARN("[arm interactive markers] TF exception:\n%s", ex.what());
      return;
    }
      
    tf::poseTFToMsg(poseout, target_pose);
}


bool resetMarker(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
	
	resetMarker();
	
	return true;
}

void resetMarker()
{
    geometry_msgs::Pose arm_pose;
    geometry_msgs::Pose blank_pose;
    blank_pose.orientation.w = 1;
    
    getTransformedPose(tip_link, blank_pose, root_link, arm_pose, ros::Time(0));
    
    server.setPose("arm_marker", arm_pose);
    server.applyChanges();
}


void changeMarkerColor(double r, double g, double b)
{
    visualization_msgs::InteractiveMarker int_marker;
    server.get("arm_marker", int_marker);
    
    visualization_msgs::Marker *box_marker = &int_marker.controls[0].markers[0];
    
    box_marker->color.r = r;
    box_marker->color.g = g;
    box_marker->color.b = b;
    
    server.insert(int_marker);
    server.applyChanges();
}

};



int main(int argc, char** argv)
{
	
  ros::init(argc, argv, "wam_arm_marker_server");
  WAMArmMarkerServer wamArmMarkerServer;
  
  ros::spin();
}
